#ifndef __MAIN_H__
#define __MAIN_H__

#include <rtthread.h>
#include <rtdevice.h>
#include <board.h>
//紫黄绿蓝
#define THREAD_PRIORITY 1
#define THREAD_STACK_SIZE 512
#define THREAD_TIMESLICE 10

#define MOTOR_PWM_PERIOD (20000000) /* 周期为20ms，单位为纳秒ns */
#define MOTOR_PWM_MIN (1000000)
#define MOTOR_PWM_MAX (2000000)

#ifdef __cplusplus
extern "C"
{
#endif

    enum
    {
        MOTOR_INDEX_1,
        MOTOR_INDEX_2,
        MOTOR_INDEX_3,
        MOTOR_INDEX_4,
    };

    struct balance_info
    {
        int request_unlock;
        int is_unlock;
        float power;
        float roll_p; //外环PID
        float roll_i;
        float pitch_p;
        float pitch_i;
        float roll_p_in; //内环PID
        float roll_i_in;
        float roll_d_in;
        float pitch_p_in;
        float pitch_i_in;
        float pitch_d_in;
    };

    struct balance_control
    {
        float roll_target; //目标角度
        float pitch_target;
        float yaw_target;

        float roll_current; // 当前角度
        float pitch_current;
        float yaw_current;

        float roll_out; // PID计算角度输出
        float pitch_out;
        float yaw_out;

        float roll_w_current; // 当前角速度
        float pitch_w_current;
        float yaw_w_current;

        rt_uint32_t throttle; //油门

        rt_uint32_t motor_pwm_out1; //四个电机输出
        rt_uint32_t motor_pwm_out2;
        rt_uint32_t motor_pwm_out3;
        rt_uint32_t motor_pwm_out4;
    };

    struct vector_3d
    {
        float x;
        float y;
        float z;
    };

    extern struct balance_info info;
    extern struct balance_control control;

    void motor_lock();
    void motor_unlock();
    void set_motor_pwm(rt_int32_t pwm1, rt_int32_t pwm2, rt_int32_t pwm3, rt_int32_t pwm4);

#ifdef __cplusplus
}
#endif

#endif /* __MAIN_H__ */